///////////////////////////////////////////////////////////
// 
//  MySimulation.cpp
// 
//  Authors:  Jeremy Bennett
//
///////////////////////////////////////////////////////////

// Local Includes
#include "stdafx.h"          // Required PCH
#include "MySimulation.h"    //

// System Includes
#include <stdio.h>
#include <iostream>          // Required File IO
#include <algorithm>         // Required Sort

#define PI 3.141592653589793238

/////
//
// * Need method for initializing simulation
//     - Select puck
//     - Select babble / test
// * Babble 
//     - Setup simulation
//     - Babble
//         - Select random action ( push direction, push location )
//            - Need to ensure at least 1 successful action per puck orientation
//         - Determine and store results
//     - Reset when out of bounds
//     - Need ability to serialize/deseralize results to/from a file
// * Test 
//     - Setup simulation
//         - Load and optimize data sets
//            - Store results based upon initial orientation
//            - Sort based movement direction relative to center of puck
//            - Sort based on rotation
//     - Place puck
//     - Place shape marker defined as a position and orientation
//     - Execute 
//          - Select action as described in flow chart
//          - Update results based upon action
//     - Reset when complete or out of bounds 
//     - Track validity
// * Play
//     - Current mode of operation
//
/////




void MySimulation::reset( unsigned int iObj )
{
    _iCurObj = iObj;
    _vAct.clear();
    _vRes.clear();

	_bOpt = false;

    _vRes.reserve(600000);
}

bool MySimulation::addResults( const obj_act &rAct, const obj_res &rRes )
{
#if 0
    assert( rAct.iOrient >= 0 && rAct.iOrient < 360 );
    assert( rAct.iPos >= 0    && rAct.iPos < 360    );
    assert( rAct.iDir >= 0    && rAct.iDir < 360    );
    
    assert( rRes.fMotAng >= 0 && rRes.fMotAng < 360 );
    assert( rRes.fOriAng >= 0 && rRes.fOriAng < 360 );
#else
    if ( !(rAct.iOrient >= 0 && rAct.iOrient < 360) ||
         !(rAct.iPos >= 0    && rAct.iPos < 360)    ||
         !(rAct.iDir >= 0    && rAct.iDir < 360)    ||
         !(rRes.fMotAng >= 0 && rRes.fMotAng < 360) ||
         !(rRes.fOriAng >= 0 && rRes.fOriAng < 360) )
        return false;
#endif

    std::set<obj_act,obj_act>::iterator iterAct = _vAct.find(rAct);
    if ( iterAct == _vAct.end() ) {
        _vAct.insert(rAct);
        iterAct = _vAct.find(rAct);
        iterAct->iNumAct = 0;
        iterAct->iSucAct = 0;
    }

    // Update the number of times the action has been added
    iterAct->iNumAct+=1; // Evil

    if ( rRes.isValid() ) {
        std::vector<obj_res>::iterator iterRes = _vRes.insert(_vRes.end(),rRes);
        iterRes->iterAct = iterAct;        
        iterAct->iSucAct+=1; // Evil
        return true;
    } else {
        return false;
    }
}

float _dist2( float fMDis, float fMAng, float fADis, float fAng )
{
    float x1 = fMDis*std::cosf(fMAng*PI/180.0f),
          y1 = fMDis*std::sinf(fMAng*PI/180.0f),
          x0 = fADis*std::cosf(fAng*PI/180.0f),
          y0 = fADis*std::sinf(fAng*PI/180.0f);
    return pow(x1-x0,2) + pow(y1-y0,2);
}

const obj_res& MySimulation::obtainClosetResult( unsigned int     iOrient,           // Current orientation 
                                                 float            fMarkerOri,        // Orientation to the marker                                              
                                                 float            fMarkerDis,        // Distance to the marker   
                                                 float            fMarkerAng) const  // Angle to the marker
{

	((MySimulation*)this)->_optimizeResults();

    unsigned int iS      = _aObjOri[ iOrient==0 ? iOrient : iOrient-1],
                 iE      = _aObjOri[ iOrient==359 ? iOrient+1 : iOrient+2],
                 iC      = (iS+iE)/2;
    float        fPerAng = fMarkerOri/float(_iMaxAng),
                 fPerDis = fMarkerDis/float(_iMaxDis);

    

    bool bLinear = true;
    if ( bLinear ) {

        const float fSwitchDis = 2.0f;

        if ( fMarkerDis > fSwitchDis ) {
            iC = iS;
            for ( unsigned int i=iS+1 ; i<iE ; i++ ) {

				unsigned int iSuc = _vResMot[i].iterAct->iSucAct,
					         iNum = _vResMot[i].iterAct->iNumAct;
				if ( iSuc < iNum/2 )
					continue;

                float fMot1 = _dist2(fMarkerDis,fMarkerAng,_vResMot[iC].fMotDis,_vResMot[iC].fMotAng),
                      fMot2 = _dist2(fMarkerDis,fMarkerAng,_vResMot[i].fMotDis,_vResMot[i].fMotAng);
                if ( fMot2 < fMot1 )
                    iC = i;
            }
            return _vResMot[iC];
        } else {


            if ( fPerAng > fPerDis ) {
                float fMinAngle = 360.0f;

                iC = iS;
                for ( unsigned int i=iS+1 ; i<iE ; i++ ) {
                    float fMot1 = _dist2(fMarkerDis,fMarkerAng,_vResMot[iC].fMotDis,_vResMot[iC].fMotAng),
                          fMot2 = _dist2(fMarkerDis,fMarkerAng,_vResMot[i].fMotDis,_vResMot[i].fMotAng);
                    float fAng1 = abs(fMarkerOri-_vResMot[iC].fOriAng),
                          fAng2 = abs(fMarkerOri-_vResMot[i].fOriAng);
               	
                    unsigned int iSuc = _vResMot[i].iterAct->iSucAct,
				    iNum = _vResMot[i].iterAct->iNumAct;
				    if ( iSuc < iNum/2 )
					    continue;

                    fMinAngle = std::min( fMinAngle, std::min( fAng1, fAng2 ) );
                    if ( fAng1 > fMinAngle+5.0f ) {
                        iC = i;
                    } else if ( fAng2 <= fMinAngle+5.0f && fMot2 < fMot1 ) {
                        iC = i;
                    }
                }
            } else {
                float fMinDis = 360.0f;

                iC = iS;
                for ( unsigned int i=iS+1 ; i<iE ; i++ ) {
                    float fMot1 = _dist2(fMarkerDis,fMarkerAng,_vResMot[iC].fMotDis,_vResMot[iC].fMotAng),
                          fMot2 = _dist2(fMarkerDis,fMarkerAng,_vResMot[i].fMotDis,_vResMot[i].fMotAng);
                    float fAng1 = abs(fMarkerOri-_vResMot[iC].fOriAng),
                          fAng2 = abs(fMarkerOri-_vResMot[i].fOriAng);
               	
                    unsigned int iSuc = _vResMot[i].iterAct->iSucAct,
				    iNum = _vResMot[i].iterAct->iNumAct;
				    if ( iSuc < iNum/2 )
					    continue;

                    fMinDis = std::min( fMinDis, std::min( fMot1, fMot2 ) );
                    if ( fMot1 > fMinDis+0.3f ) {
                        iC = i;
                    } else if ( fMot2 <= fMinDis+0.3f && fAng2 < fAng1 ) {
                        iC = i;
                    }
                }

            }
#if 0
            printf( "Orient: %d|%.2f Position: %.2f@%.2f Action: %.2f-%.2f@%.2f\n", 
                     iOrient, fMarkerOri, fMarkerDis, fMarkerAng, _vResMot[iC].fOriAng, _vResMot[iC].fMotDis, _vResMot[iC].fMotAng );
#endif
            return _vResMot[iC];
        } 

    } else {
        obj_res or(fMarkerDis,fMarkerAng,fMarkerOri);

        std::vector<obj_res>::const_iterator iterRes;

        bool bNearDist = fPerDis < fPerAng;
        if ( bNearDist ) {    
            std::vector<obj_res>::const_iterator iterResS =  _vResMot.begin(),
                                                 iterResE =  _vResMot.begin();
            iterResS+=iS;
            iterResE+=iE;
            iterRes = std::lower_bound( iterResS, iterResE, or, obj_res::lessMot );

        } else {
            std::vector<obj_res>::const_iterator iterResS =  _vResOri.begin(),
                                                 iterResE =  _vResOri.begin();
            iterResS+=iS;
            iterResE+=iE;
            iterRes = std::lower_bound( iterResS, iterResE, or, obj_res::lessOri );
        }
    
        return *iterRes;
    }
}

void MySimulation::serialize( const char* fname )
{
    FILE *fp = fopen( fname, "w" );
    if ( fp ) {
        
        unsigned int iObj = _iCurObj,
                     nAct = _vAct.size(),
                     nRes = _vRes.size();
        
        // Output header
        fprintf( fp, "Obj: %d  Act: %d  Res: %d\n", iObj, nAct, nRes );

        // Output Actions
        std::set<obj_act,obj_act>::iterator iterAct = _vAct.begin();
        do {
            fprintf( fp, 
                     "Ori: %d  Pos: %d  Dir: %d  Dis: %d  Num: %d  Suc: %d\n",  
                     iterAct->iOrient, 
                     iterAct->iPos, 
                     iterAct->iDir, 
                     iterAct->iDis, 
                     iterAct->iNumAct, 
                     iterAct->iSucAct );
        } while ( ++iterAct != _vAct.end() );

		if ( nRes ) {
			// Output results
			std::vector<obj_res>::iterator iterRes = _vRes.begin();
			int i=0;
			do {
				fprintf( fp,
						 "MotDis: %f  MotAng: %f  OriAng: %f  "
						 "Ori: %d  Pos: %d  Dir: %d  Dis: %d\n",
						 iterRes->fMotDis,
						 iterRes->fMotAng,
						 iterRes->fOriAng,
						 iterRes->iterAct->iOrient, 
						 iterRes->iterAct->iPos, 
						 iterRes->iterAct->iDir, 
						 iterRes->iterAct->iDis );
				i++;
			} while ( ++iterRes != _vRes.end() );
		}


        fclose(fp);
    } else {
        std::cout << "MySimulation::Serialize(...) - Failed to serialize file: " << fname << std::endl;
    }
}

void MySimulation::deserialize( const char* fname )
{
	_bOpt = false;

	//std::cout<<"\n\n\nDeserializing...\n\n\n";
    FILE *fp = fopen( fname, "r" );
    if ( fp ) {

        std::set<obj_act,obj_act>::iterator iterAct;

        obj_act      rAct;
        obj_res      rRes;
        unsigned int n    = 0,
                     iObj = 0,
                     nAct = 0,
                     nRes = 0;

        // Input header
        fscanf( fp, "Obj: %d  Act: %d  Res: %d\n", &iObj, &nAct, &nRes );

        // Input results
        for ( n=0 ; n<nAct ; n++ ) {
            fscanf( fp, 
                     "Ori: %d  Pos: %d  Dir: %d  Dis: %d  Num: %d  Suc: %d\n",  
                     &rAct.iOrient, 
                     &rAct.iPos, 
                     &rAct.iDir, 
                     &rAct.iDis, 
                     &rAct.iNumAct, 
                     &rAct.iSucAct );

            _vAct.insert(rAct);
        }

        _vRes.reserve(nRes);
        // Input action
        for ( n=0 ; n<nRes ; n++ ) {
            fscanf( fp,
                     "MotDis: %f  MotAng: %f  OriAng: %f  "
                     "Ori: %d  Pos: %d  Dir: %d  Dis: %d\n",
                     &rRes.fMotDis,
                     &rRes.fMotAng,
                     &rRes.fOriAng,
                     &rAct.iOrient, 
                     &rAct.iPos, 
                     &rAct.iDir, 
                     &rAct.iDis );

            std::vector<obj_res>::iterator iterRes = _vRes.insert(_vRes.end(),rRes);
            iterRes->iterAct = _vAct.find(rAct);
        }

        fclose(fp);
    } else {
        std::cout << "MySimulation::Serialize(...) - Failed to deserialize file: " << fname << std::endl;
    }
}



bool MySimulation::readFile()
{
	  FILE *fp = fopen( "shaftu.txt", "r" );
	  config con;
	  float x,y,z,p,q,r;
	  std::string tempShape,tempMarker;
      if (fp) {

        int total;
				
        // Input header
        fscanf( fp, " Total: %d\n", &total );
		

		
        // Input results
		
			for ( int n=0 ; n<total ; n++ ) {
				fscanf( fp, "obj: %d  initShapePos: %f,%f,%f  initShapeAng: %f  initMarkerPos: %f,%f,%f  initMarkerAng: %f\n",  
                     &con.shape,
					 &x,&y,&z,
                     &con.initShapeAng, 
					 &p,&q,&r,
                     &con.initMarkerAng);

				
					con.aInitShapePos[0] = x;
					con.aInitShapePos[1] = y;
					con.aInitShapePos[2] = z;

				    con.aInitMarkerPos[0] = p;
					con.aInitMarkerPos[1] = q;
					con.aInitMarkerPos[2] = r;
			
				//std::cout << " aInitShapePos " << &con.aInitShapePos;
				//std::cout << "  aInitMarkerPos " << &con.aInitMarkerPos;
				//setPosition(&x,&y,&z,con, 1);
				//setPosition(&p,&q,&r,con, 2);
				_vConfig.insert(_vConfig.end(), con);
			}
	    
        fclose(fp);
		return true;
    } else {
        std::cout << "MySimulation::readFile(...) - Failed to read input file: shaftu.txt" << std::endl;
		return false;
	  }


}

void MySimulation::setPosition(float a,float b, float c, config &con , int x)
{
	/*
	int first = temp.find_first_of(",");
	int	second = temp.find_first_of("," , first+1);
	
	if (x==1) { 
	con.initShapePos = btVector3(atof(temp.substr(0,first).c_str()),
					atof(temp.substr(first+1,second).c_str()),
					atof(temp.substr(second+1).c_str()));
	} else {
	con.initMarkerPos = btVector3(atof(temp.substr(0,first).c_str()),
					atof(temp.substr(first+1,second).c_str()),
					atof(temp.substr(second+1).c_str())); 
	}
	*/
}




void MySimulation::test()
{
    // Local variables
    MySimulation mS1,      // Simulation 1: Used for serialization 
                 mS2;      // Simulation 2: Used for deserialization
    obj_act      oa;       // Object action
    obj_res      or;       // Object results
	config       co;
    int          iSuc = 0, // Test counter for successful actions
                 iNum = 0; // Test counter for number of actions

    std::set<obj_act,obj_act>::iterator iterAct; // Action iterator

    // Generate and add actions
    const int nObj = 100;
    for ( int n=0 ; n<nObj ; n++ ) {
        // Randomly populate an action
        oa.iOrient =  rand() % 360;
        oa.iPos    =  rand() % 360;
        oa.iDir    =  rand() % 360;
        oa.iDis    =  rand() % 360;
        
        // Randomly populate a result
        or.fMotDis = rand() % 360;
        or.fMotAng = rand() % 360;
        or.fOriAng = rand() % 360;
    
        // Add to the 1st simulation
        mS1.addResults( oa, or );
    }

    // Verifty the correct number of successful actions were added
    iterAct = mS1._vAct.begin();
    do {
        // Update the number of actions and success
        iSuc += iterAct->iSucAct;
        iNum += iterAct->iNumAct;
    } while( ++iterAct != mS1._vAct.end() );

    // Check the correct number of actions are recorded
    if ( iSuc != nObj || iNum != nObj ) {
        std::cout << "MySimulation::test() - Invalid number of adds!" << std::endl;
    }

    // Setup a new action 
    oa.iOrient =  0;
    oa.iPos    =  0;
    oa.iDir    =  0;
    oa.iDis    =  5;

    // Setup a new results
    or.fMotDis = 1.0f;
    or.fMotAng = 1.0f;
    or.fOriAng = 1.0f;

    // Add a results for the new action
    mS1.addResults( oa, or );

    // Check the action is present
    iterAct = mS1._vAct.find(oa);
    if ( (oa == *iterAct) != true ) {
         std::cout << "MySimulation::test() - Could not find action!" << std::endl;
    }

    // Check the action is updated
    if ( iterAct->iSucAct != 1 || iterAct->iNumAct != 1 ) {
        std::cout << "MySimulation::test() - Action not correct!" << std::endl;
    }
    
    // Check the results are present
    if ( (mS1._vRes.size() != nObj+1)    || 
         (or == mS1._vRes[nObj]) != true ||
         (mS1._vRes[nObj].iterAct == iterAct) != true ) {
         std::cout << "MySimulation::test() - Could not find result!" << std::endl;
    }

    // Setup a new result
    or.fOriAng    = 1.1f;
    mS1.addResults( oa, or );

    // Check the action is updated
    if ( iterAct->iSucAct != 2 || iterAct->iNumAct != 2 ) {
        std::cout << "MySimulation::test() - Action not correct!" << std::endl;
    }

    // Check the result is present
    if ( (mS1._vRes.size() != nObj+2)      || 
         (or == mS1._vRes[nObj+1]) != true ||
         (mS1._vRes[nObj+1].iterAct == iterAct) != true ) {
         std::cout << "MySimulation::test() - Could not find result!" << std::endl;
    }

    // Setup up an invalid result
    or.fMotDis = 0.0f;
    or.fMotAng = 0.0f;
    or.fOriAng = 0.0f;
    mS1.addResults( oa, or );

    // Check the action is updated
    if ( iterAct->iSucAct != 2 || iterAct->iNumAct != 3 ) {
        std::cout << "MySimulation::test() - Action not9 correct!" << std::endl;
    }

    // Check the result is not present
    if ( mS1._vRes.size() != nObj+2 ) {
        std::cout << "MySimulation::test() - Too many results found!" << std::endl;
    }
    
    mS1.serialize("foo.txt");
    mS2.deserialize("foo.txt");

    if ( mS1._vAct.size() != mS2._vAct.size() || 
         mS1._vRes.size() != mS2._vRes.size()    ) {
        std::cout << "MySimulation::test() - Failed to read correct number" << std::endl;
    } else {
        std::set<obj_act,obj_act>::iterator iterAct1 = mS1._vAct.begin(),
                                            iterAct2 = mS2._vAct.begin();
        std::vector<obj_res>::iterator      iterRes1 = mS1._vRes.begin(),
                                            iterRes2 = mS2._vRes.begin();
        
        // Verify the actions are identical
        do {
            if ( (*iterAct1 == *iterAct2) == false ) {
                std::cout << "MySimulation::test() - Failed to read correct action" << std::endl;
            }
        } while ( ++iterAct1 != mS1._vAct.end() && ++iterAct2 != mS2._vAct.end() ) ;

        // Verify the results are identical
        do {
            if ( (*iterRes1 == *iterRes2) == false ) {
                std::cout << "MySimulation::test() - Failed to read correct result" << std::endl;
            }
        } while ( ++iterRes1 != mS1._vRes.end() && ++iterRes2 != mS2._vRes.end() );
    }

    {
        mS1._optimizeResults();

        for ( unsigned int n=0 ; n<360 ; n++ ) {
            unsigned int iS = mS1._aObjOri[n],
                         iE = mS1._aObjOri[n+1];
            while( iS < iE ) {
                if ( mS1._vResMot[iS].iterAct->iOrient != n )
                    std::cout << "MySimulation::test() - Failed to optimize!" << std::endl;
                iS++;
            }
        }
    }
}

void MySimulation::test2()
{
    // Local variables
    MySimulation mS; // Simulation
    obj_res      or; // Object results

    std::set<obj_act,obj_act>::iterator iterAct; // Action iterator

    const int nObj = 4;

    { obj_act oa(0,0,0,0); obj_res or(5,180,  0); mS.addResults(oa,or); }
    { obj_act oa(0,1,0,0); obj_res or(3,180,  0); mS.addResults(oa,or); }
    { obj_act oa(0,2,0,0); obj_res or(5,180, 90); mS.addResults(oa,or); }
    { obj_act oa(0,3,0,0); obj_res or(5,180,270); mS.addResults(oa,or); }
    { obj_act oa(0,4,0,0); obj_res or(5, 91,  0); mS.addResults(oa,or); }
    { obj_act oa(0,4,0,0); obj_res or(4, 90,  0); mS.addResults(oa,or); }
    { obj_act oa(0,5,0,0); obj_res or(5, 89,  0); mS.addResults(oa,or); }

    mS._optimizeResults();
        
    or = mS.obtainClosetResult(0,0,5,180);
    assert( or.fMotAng == 180 && or.fMotDis == 5 );

    or = mS.obtainClosetResult(0,0,3,180);
    assert( or.fMotAng == 180 && or.fMotDis == 3 );

    or = mS.obtainClosetResult(0,0,5,90);
    assert( or.fMotAng == 91 && or.fMotDis == 5 );

    or = mS.obtainClosetResult(0,90,1,180);
    assert( or.fMotAng == 180 && or.fMotDis == 5 && or.fOriAng == 90 );
}

bool res_less_ori_func( const obj_res& lhs, const obj_res& rhs ) 
  { return obj_res::lessOri(lhs,rhs); }

bool res_less_mot_func( const obj_res& lhs, const obj_res& rhs ) 
  { return obj_res::lessMot(lhs,rhs); }

void MySimulation::_optimizeResults()
{
    if ( _bOpt == false  && _vRes.size() > 0 ) {

		_vResMot.clear();
		_vResOri.clear();
        memset(_aObjOri,0,sizeof(unsigned int)*360);

        _vResMot = _vRes;
        _vResOri = _vRes;
        
        std::sort( _vResMot.begin(), _vResMot.end(), res_less_mot_func );
        std::sort( _vResOri.begin(), _vResOri.end(), res_less_ori_func );

        unsigned int iCurAng=0, iIdx=0;

        _aObjOri[0] = 0;

        std::vector<obj_res>::iterator iterRes = _vResMot.begin();
        do {
            unsigned int iOriAng = iterRes->iterAct->iOrient;
            if ( iOriAng != iCurAng ) {
                for ( unsigned int n=iCurAng+1 ; n<=iOriAng ; n++ ) {
                    _aObjOri[n] = iIdx;
                    iCurAng = iOriAng;
                }
            }
            iIdx++;
        } while ( ++iterRes != _vResMot.end() );
        
        // Update the remaining orientations
        for ( int n=iCurAng+1 ; n<=360 ; n++ ) {
            _aObjOri[n] = iIdx;
        }

        _bOpt = true;
    }
}